#include <QGraphicsLineItem>
#include <QMouseEvent>
#include <QPolygonF>
#include <QPainter>
#include <QDebug>
#include "box2dwidget.h"

const float PI = 3.14159265;

/*!
    \class QtBox2DContactListener

    \brief Use this class for collision detection between bodies. Add bodies
    to the listener with QtBox2DContactListener::addBody().

    \sa QtBox2DEngine::setContactListener
 */

QtBox2DContactListener::QtBox2DContactListener(QObject *parent)
    : QObject(parent)
{
}

/*!
    \reimp
 */
void QtBox2DContactListener::BeginContact(b2Contact *contact)
{
    b2Body *a = contact->GetFixtureA()->GetBody();
    b2Body *b = contact->GetFixtureB()->GetBody();
    QtBox2DBody *ab = static_cast<QtBox2DBody *>(a->GetUserData());
    QtBox2DBody *bb = static_cast<QtBox2DBody *>(b->GetUserData());
    QList<QtBox2DBody *>::const_iterator i;
    for (i = m_bodies.constBegin(); i != m_bodies.constEnd(); ++i) {
        const b2Body *body = (*i)->body();
        if (a == body) {
            emit QtBox2DContactListener::contact(bb, ab);
        } else if (b == body) {
            emit QtBox2DContactListener::contact(ab, bb);
        }
    }
}

/*!
    \fn void QtBox2DContactListener::addBody(QtBox2DBody *body)

    Adds a body to the contact listener.
 */

/*!
    \class QtBox2DFixture

    \brief A wrapper for the b2Fixture class with additional properties
    required to provide a visual representation of Box2D shapes on a
    QtBox2DWidget.

    \sa QtBox2DWidget
 */

/*!
    \enum QtBox2DFixture::Box2DShapeType

    \value CircleShape

    \value EdgeShape

    \value PolygonShape

    \value ChainShape
 */

/*!
    Constructs a QtBox2DFixture.
 */
QtBox2DFixture::QtBox2DFixture(b2Fixture *fixture)
    : m_fixture(fixture),
      m_visible(true)
{
}

/*!
    Destroys the QtBox2DFixture object.
 */
QtBox2DFixture::~QtBox2DFixture()
{
    void *data = m_fixture->GetUserData();
    QGraphicsItem *item = static_cast<QGraphicsItem *>(data);
    delete item;
}

/*!
    Returns the type of this fixture.
 */
QtBox2DFixture::Box2DShapeType QtBox2DFixture::shapeType() const
{
    return static_cast<Box2DShapeType>(m_fixture->GetShape()->GetType());
}

/*!
    \fn void QtBox2DFixture::setFriction(float32 friction)
 */

/*!
    \fn float32 QtBox2DFixture::friction() const
 */

/*!
    \fn void QtBox2DFixture::setDensity(float32 density)
 */

/*!
    \fn float32 QtBox2DFixture::density() const
 */

/*!
    \fn void QtBox2DFixture::setRestitution(float32 restitution)
 */

/*!
    \fn float32 QtBox2DFixture::restitution() const
 */

/*!
    \fn void QtBox2DFixture::setVisible(bool visible)
 */

/*!
    \fn bool QtBox2DFixture::isVisible() const
 */

/*!
    \fn const b2Shape *QtBox2DFixture::shape() const
 */

/*!
    \fn void QtBox2DFixture::setBrush(const QBrush &brush)
 */

/*!
    \fn QBrush QtBox2DFixture::brush() const
 */

/*!
    \fn void QtBox2DFixture::setPen(const QPen &pen)
 */

/*!
    \fn QPen QtBox2DFixture::pen() const
 */

/*!
    \class QtBox2DBody

    \brief A Box2D body which can be rendered on a QtBox2DWidget.

    Bodies are the backbone for fixtures. Bodies carry fixtures and move them
    around in the world. Bodies are always rigid bodies in Box2D. That means
    that two fixtures attached to the same rigid body never move relative to
    each other.

    \sa QtBox2DFixture
 */

/*!
    \enum QtBox2DBody::Box2DBodyType

    \value StaticBody  A static body does not move under simulation and behaves
    as if it has infinite mass.

    See the Box2D documentation.

    \value KinematicBody  A kinematic body moves under simulation according to
    its velocity. Kinematic bodies do not respond to forces. They can be moved
    manually by the user, but normally a kinematic body is moved by setting its
    velocity.

    See the Box2D documentation.

    \value DynamicBody  A dynamic body is fully simulated. They can be moved
    manually by the user, but normally they move according to forces. A dynamic
    body can collide with all body types. A dynamic body always has finite,
    non-zero mass.

    See the Box2D documentation.
 */

/*!
    \enum QtBox2DBody::Box2dBodyPaintFlag

    Flags used to determine how items are painted on a QtBox2DWidget.

    \value PaintRotation  If active, shapes are painted rotated to their
    actual rotation. Otherwise, items are drawn at a constant angle.
    This flag is enabled by default.

    \value DebugDraw  When this flag is enabled, some additional shape
    elements are visible, such as the radius of circles.
 */

/*!
    Constructs a QtBox2DBody object of specified \a type. QtBox2DBody contains
    a Box2D body that can be drawn on a QtBox2DWidget.
 */
QtBox2DBody::QtBox2DBody(QtBox2DEngine *engine, Box2DBodyType type)
    : m_engine(engine),
      m_body(m_engine->createBody(static_cast<b2BodyType>(type))),
      m_flags(PaintRotation)
{
    m_body->SetUserData(this);
}

/*!
    Destroys this QtBox2DBody object.
 */
QtBox2DBody::~QtBox2DBody()
{
    m_engine->destroyBody(m_body);
    QList<QtBox2DFixture *>::const_iterator i;
    for (i = m_fixtures.constBegin(); i != m_fixtures.constEnd(); ++i)
        delete *i;
}

/*!
    \fn QList<QtBox2DFixture *> QtBox2DBody::fixtures() const

    Returns a list of this body's fixtures.
 */

/*!
    Sets the position of this body to coordinates \a x and \a y.
 */
void QtBox2DBody::setPosition(qreal x, qreal y)
{
    if (m_engine->worldLocked()) {
        qWarning("Cannot set body position within timestep.");
        return;
    }
    m_body->SetTransform(b2Vec2(x, -y), m_body->GetAngle());
}

/*!
    \fn QPointF QtBox2DBody::position() const

    Returns the position of this body.
 */

/*!
    Sets the rotation angle of this body to \a r degrees.
 */
void QtBox2DBody::setRotation(qreal r)
{
    if (m_engine->worldLocked()) {
        qWarning("Cannot set body rotation within timestep.");
        return;
    }
    float32 angle = r ? -180 * PI / r : 0;
    m_body->SetTransform(m_body->GetPosition(), angle);
}

/*!
    Returns the angle of the body in degrees.
 */
qreal QtBox2DBody::rotation() const
{
    return -(m_body->GetAngle() * 180) / PI;
}

/*!
    Sets the type of this body.
 */
void QtBox2DBody::setType(Box2DBodyType type)
{
    if (m_engine->worldLocked()) {
        qWarning("Cannot change body type within timestep.");
        return;
    }
    m_body->SetType(static_cast<b2BodyType>(type));
}

/*!
    \fn void QtBox2DBody::setActive(bool flag)

    Set the activation state of the body. See the Box2D documentation.
 */

/*!
    \fn bool QtBox2DBody::isActive() const

    Returns true if the body is active.
 */

/*!
    \fn void QtBox2DBody::setAngularDamping(float32 damping)

    Damping is used to reduce the world velocity of bodies. Damping is not a
    replacement for friction and the two effects should be used together.
    Damping parameters should be between 0 and infinity, with 0 meaning no
    damping, and infinity meaning full damping. Normally you will use a
    damping value between 0 and 0.1. At small damping values the damping
    effect is mostly independent of the time step. See Box2D's documentation.
 */

/*!
    \fn float32 QtBox2DBody::angularDamping() const

    Returns the body's angular damping.
 */

/*!
    \fn void QtBox2DBody::setAngularVelocity(float32 velocity)
 */

/*!
    \fn float32 QtBox2DBody::angularVelocity() const

    Returns the body's angular velocity.
 */

/*!
    \fn void QtBox2DBody::setBullet(bool flag)

    In Box2D, continuous collision detection (CCD) is used to prevent dynamic
    bodies from tunneling through static ones. This is done by sweeping shapes
    from their old position to their new positions. Fast moving objects can be
    labeled as bullets. Bullets will perform CCD with both static and dynamic
    bodies. The bullet flag only affects dynamic bodies.
 */

/*!
    \fn bool QtBox2DBody::isBullet() const

    Returns true if the body is a bullet.
 */

/*!
    \fn void QtBox2DBody::setLinearDamping(float32 damping)
 */

/*!
    \fn float32 QtBox2DBody::linearDamping() const
 */

/*!
    \fn void QtBox2DBody::setLinearVelocity(float32 x, float32 y)
 */

/*!
    \fn QVector2D QtBox2DBody::linearVelocity() const
 */

/*!
    \fn void QtBox2DBody::applyAngularImpulse(float32 impulse)
 */

/*!
    \fn void QtBox2DBody::applyForce(float32 x, float32 y, float32 px, float32 py)

    Apply a force with vector coordinates \a x, \a y to the body at point
    \a px, \a py.
 */

/*!
    \fn void QtBox2DBody::applyForceToCenter(float32 x, float32 y)

    Apply a force with vector coordinates \a x, \a y to the body's center.
 */

/*!
    \fn void QtBox2DBody::applyLinearImpulse(float32 x, float32 y, float32 px, float32 py)
 */

/*!
    \fn void QtBox2DBody::applyTorque(float32 torque)
 */

/*!
    \fn const b2Body *QtBox2DBody::body() const

    Returns a const pointer to the b2Body object.
 */

/*!
    Assign flags used to determine how this body is painted.

    \sa Box2dBodyPaintFlag
 */
void QtBox2DBody::setPaintFlag(Box2dBodyPaintFlag flag, bool active)
{
    active ? m_flags |= flag : m_flags ^= flag;
}

/*!
    \fn int QtBox2DBody::flags() const
    \sa Box2dBodyPaintFlag

    Returns the body's current paint flags.
 */

QtBox2DFixture *QtBox2DBody::createFixture(const b2Shape &shape)
{
    QtBox2DFixture *fixture = new QtBox2DFixture(m_engine->createFixture(m_body, &shape));
    m_fixtures.push_back(fixture);
    return fixture;
}

/*!
    \class QtBox2DWidget

    \brief Provides a visual representation of a QtBox2DEngine simulation and
    manages painting of all b2World items.
 */

/*!
    Constructs a QtBox2DWidget object for \a engine.
 */
QtBox2DWidget::QtBox2DWidget(QtBox2DEngine *engine, QWidget *parent)
    : QWidget(parent),
      m_engine(engine),
      m_mouseJoint(0),
      m_mouseBody(engine->createBody(b2_staticBody)),
      m_rubberband(false),
      m_bandEnabled(false),
      m_timer(startTimer(1000/60)),
      m_scale(10)
{
}

/*!
    \fn QtBox2DWidget::~QtBox2DWidget()

    Destroys the QtBox2DWidget.
 */

/*!
    \fn void QtBox2DWidget::setScale(qreal s)

    Sets the scale factor used when painting the simulation.
 */

/*!
    Returns the current QTransform object used when painting the simulation.
 */
QTransform QtBox2DWidget::transform() const
{
    QTransform t;
    t.translate(width()/2, height()/2);
    t.scale(m_scale, m_scale);
    return t;
}

/*!
    \fn void QtBox2DWidget::setRubberbandEnabled(bool enabled)
 */

/*!
    Creates a circle fixture on \a body with center \a x, \a y and \a radius.
 */
QtBox2DFixture *QtBox2DWidget::createCircleFixture(QtBox2DBody *body, float radius, float x, float y)
{
    b2CircleShape circle;
    circle.m_p.Set(x, y);
    circle.m_radius = radius;
    return body->createFixture(circle);
}

/*!
    Creates a edge fixture on \a body between points \a x1, \a y1 and \a x2, \a y2.
 */
QtBox2DFixture *QtBox2DWidget::createEdgeFixture(QtBox2DBody *body, float x1, float y1, float x2, float y2)
{
    b2EdgeShape edge;
    edge.Set(b2Vec2(x1, y1), b2Vec2(x2, y2));
    return body->createFixture(edge);
}

/*!
    Creates a polygon fixture on \a body, as defined by \a polygon.
 */
QtBox2DFixture *QtBox2DWidget::createPolygonFixture(QtBox2DBody *body, const QPolygonF &polygon)
{
    int count = polygon.count();
    b2Vec2 v[count];
    insertVertices(polygon, v);
    b2PolygonShape shape;
    shape.Set(v, count);
    return body->createFixture(shape);
}

/*!
    Creates a chain fixture on \a body, as defined by \a polygon.
 */
QtBox2DFixture *QtBox2DWidget::createChainFixture(QtBox2DBody *body, const QPolygonF &polygon)
{
    int count = polygon.count();
    b2Vec2 v[count];
    insertVertices(polygon, v);
    b2ChainShape shape;
    shape.CreateChain(v, count);
    return body->createFixture(shape);
}

/*!
    \reimp
 */
void QtBox2DWidget::paintEvent(QPaintEvent *)
{
    QPainter painter(this);
    painter.setTransform(transform());
    b2Body *body = m_engine->bodies();
    QtBox2DBody *b;
    while (body) {
        if ((b = static_cast<QtBox2DBody *>(body->GetUserData())))
            paintFixtures(&painter, b);
        body = body->GetNext();
    }
    if (m_rubberband && m_bandEnabled)
        painter.drawLine(m_line);
}

/*!
    \reimp
 */
void QtBox2DWidget::mousePressEvent(QMouseEvent *event)
{
    if (!m_engine || !m_bandEnabled)
        return;
    QtBox2DQueryCallback cb(b2_dynamicBody);
    cb.setTerminate(true);
    m_mpPos = transform().inverted().map(event->pos());
    qreal x = m_mpPos.x();
    qreal y = m_mpPos.y();
    connect(&cb, SIGNAL(bodyFound(b2Body*)), this, SLOT(createRubberband(b2Body*)));
    QPointF tl = QPointF(x, -y);
    QPointF br = tl + QPointF(0.3, 0.3);
    m_engine->query(&cb, tl, br);
    m_line.setLine(x, y, x, y);
}

/*!
    \reimp
 */
void QtBox2DWidget::mouseReleaseEvent(QMouseEvent*)
{
    if (!m_rubberband)
        return;
    if (m_mouseJoint) {
        m_engine->destroyJoint(m_mouseJoint);
        m_mouseJoint = 0;
    }
    m_rubberband = false;
    update();
}

/*!
    \reimp
 */
void QtBox2DWidget::timerEvent(QTimerEvent *event)
{
    if (m_rubberband && m_mouseJoint && m_timer == event->timerId()) {
        QPointF p = transform().inverted().map(mapFromGlobal(QCursor::pos()));
        m_mouseJoint->SetTarget(b2Vec2(p.x(), -p.y()));
        const b2Vec2 &a = m_mouseJoint->GetAnchorA();
        const b2Vec2 &b = m_mouseJoint->GetAnchorB();
        m_line.setLine(a.x, -a.y, b.x, -b.y);
    }
    update();
}

void QtBox2DWidget::createRubberband(b2Body *body)
{
    if (!m_bandEnabled)
        return;
    if (m_mouseJoint)
        m_engine->destroyJoint(m_mouseJoint);
    b2MouseJointDef jointDef;
    jointDef.bodyA = m_mouseBody;
    jointDef.bodyB = body;
    jointDef.collideConnected = false;
    jointDef.target = b2Vec2(m_mpPos.x(), -m_mpPos.y());
    jointDef.maxForce = 300 * body->GetMass();
    jointDef.frequencyHz = 10;
    m_mouseJoint = static_cast<b2MouseJoint *>(m_engine->createJoint(&jointDef));
    m_rubberband = true;
}

void QtBox2DWidget::paintFixtures(QPainter *painter, const QtBox2DBody *body) const
{
    if (!body)
        return;
    QPointF pos = body->position();
    qreal a = body->rotation();
    painter->setRenderHint(QPainter::Antialiasing);
    QList<QtBox2DFixture *> fixtures = body->fixtures();
    QList<QtBox2DFixture *>::const_iterator i;
    for (i = fixtures.constBegin(); i != fixtures.constEnd(); ++i) {
        QtBox2DFixture *fixture = *i;
        if (!fixture->isVisible())
            continue;
        const b2Shape *shape = fixture->shape();
        painter->setBrush(fixture->brush());
        painter->setPen(fixture->pen());
        switch (shape->GetType())
        {
        case b2Shape::e_circle:
        {
            const b2CircleShape *cs = static_cast<const b2CircleShape *>(shape);
            float r = cs->m_radius;
            float d = r * 2;
            QRectF box(cs->m_p.x - r, -cs->m_p.y - r, d, d);
            painter->save();
            QTransform transform;
            transform.translate(pos.x(), pos.y());
            if (body->flags() & QtBox2DBody::PaintRotation)
                transform.rotate(a);
            painter->setTransform(transform, true);
            painter->drawEllipse(box);
            if (body->flags() & QtBox2DBody::DebugDraw)
                painter->drawLine(QPointF(cs->m_p.x, -cs->m_p.y), QPointF(cs->m_p.x + r, -cs->m_p.y));
            painter->restore();
            break;
        }
        case b2Shape::e_polygon:
        {
            const b2PolygonShape *ps = static_cast<const b2PolygonShape *>(shape);
            QPolygonF p;
            for (int i = 0; i < ps->GetVertexCount(); ++i) {
                b2Vec2 vertex = ps->GetVertex(i);
                p.push_back(QPointF(vertex.x, -vertex.y));
            }
            painter->save();
            QTransform transform;
            transform.translate(pos.x(), pos.y());
            if (body->flags() & QtBox2DBody::PaintRotation)
                transform.rotate(a);
            painter->setTransform(transform, true);
            painter->drawPolygon(p);
            painter->restore();
            break;
        }
        case b2Shape::e_edge:
        {
            const b2EdgeShape *es = static_cast<const b2EdgeShape *>(shape);
            b2Vec2 v1 = es->m_vertex1;
            b2Vec2 v2 = es->m_vertex2;
            painter->save();
            QTransform transform;
            transform.translate(pos.x(), pos.y());
            if (body->flags() & QtBox2DBody::PaintRotation)
                transform.rotate(a);
            painter->setTransform(transform, true);
            painter->drawLine(QPointF(v1.x, -v1.y), QPointF(v2.x, -v2.y));
            painter->restore();
            break;
        }
        case b2Shape::e_chain:
        {
            const b2ChainShape *cs = static_cast<const b2ChainShape *>(shape);
            QPolygonF p;
            for (int i = 0; i < cs->GetChildCount(); ++i)
            {
                b2EdgeShape edge;
                cs->GetChildEdge(&edge, i);
                p.push_back(QPointF(edge.m_vertex1.x, -edge.m_vertex1.y));
                p.push_back(QPointF(edge.m_vertex2.x, -edge.m_vertex2.y));
            }
            painter->save();
            QTransform transform;
            transform.translate(pos.x(), pos.y());
            if (body->flags() & QtBox2DBody::PaintRotation)
                transform.rotate(a);
            painter->setTransform(transform, true);
            painter->drawPolyline(p);
            painter->restore();
            break;
        }
        case b2Shape::e_typeCount:
        default:
            break;
        } // end switch
    }
}

void QtBox2DWidget::insertVertices(QPolygonF polygon, b2Vec2 *vertices)
{
    if (polygon.count() > 2) {
        // Determine whether the polygon has its vertices ordered
        // clockwise or counterclockwise
        const QPointF &p1 = polygon.at(0);
        const QPointF &p2 = polygon.at(1);
        const QPointF &p3 = polygon.at(2);
        qreal cprod = (p2.x() - p1.x()) * (p3.y() - p2.y())
                    - (p2.y() - p1.y()) * (p3.x() - p2.x());
        if (cprod > 0) {
            // Reverse the polygon
            std::vector<QPointF> v = polygon.toStdVector();
            std::reverse(v.begin(), v.end());
            polygon = QPolygonF(QPolygonF::fromStdVector(v));
        }
    }
    int count = polygon.count();
    for (int i = 0; i < count; ++i) {
        QPointF p = polygon.at(i);
        vertices[i].Set(p.x(), -p.y());
    }
}

